#include "interrupt.h"
#include "usart.h"
#include "adcc.h"
#include "adc.h"
#include "tim.h"
#include "pid.h"

int time_1s = 0;
int flag_stop = 0;
extern int flag_init;

 float angle_out;

int num_counter = 30000;

int start_mode = -11;

int raw_now = 0;

int alarm_left = 0;
int alarm_right = 0;

int a = 0;

int raw_filter;

extern int c, b;

extern int target_angle;

int time_angle;
int time_location;

int location_out;

int location_now;
int location_last;
int target_location;

int speed;

int stat1 = 1;
int stat2 = 1;

extern int flag_jc12;
extern int flag_jc3;
extern int flag_fh;

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    // 中断 == 1ms
    if (htim->Instance == TIM3)
    {
        time_angle++;
        time_location++;
        // 角度获取＋滤波
        raw_now = (int)(get_adc(&hadc1) * 1000);
        raw_filter = moveAverageFilter(raw_now);

        // 5ms 进行一次角度环输出
        if (time_angle == 5)
        {
            angle_out = 0.8 * vertical_PID_value((raw_filter - location_out), target_angle);
            time_angle = 0;
        }
        // 50ms 进行一次位置环输出
        if (time_location == 50)
        {
            // 获取位置
            location_now = __HAL_TIM_GET_COUNTER(&htim2);
            // 计算速度
            speed = (location_now - location_last);
            location_last = location_now;
            // 接近停止时，重置位置环目标值为现在位置
            if ((speed < 2 && speed > -2))
            {
                target_location = location_now;
            }
            // 边界保护
            if (location_now < 29000)
            {
                stat1 = 0;
            }
            if (stat1 == 0)
            {
                target_angle = 3446;
            }
            if (location_now > 31000)
            {
                stat2 = 0;
            }
            if (stat2 == 0)
            {
                target_angle = 3475;
            }
            // 慢慢回中点，取消边界保护
            if (location_now > 29500 && location_now < 30500)
            {
                target_angle = 3457;
                stat1 = 1;
                stat2 = 1;
            }
            // 位置环输出
            location_out = location_PID_value(location_now, target_location);
            time_location = 0;
        }
    }
    // 中断 == 10ms/////////////////////////////////////////////////////////////////////
    if (htim->Instance == TIM1)
    {

        num_counter = __HAL_TIM_GET_COUNTER(&htim2);
        // 边界保护
        if (num_counter > 32000 && a == 1)
        {
            alarm_right = 1;
            start_mode = 1;
        }
        if (num_counter < 27000 && a == 1)
        {
            alarm_left = 1;
            start_mode = 1;
        }
        if (num_counter > 29000 && num_counter < 31000)
        {

            alarm_left = 0;
            alarm_right = 0;
        }
        // 运动
        if (flag_init == 0 && flag_stop == 0)
        {
            // 基础12题，简单起摆
            if (flag_jc12 == 1)
            {
                if (raw_filter > 1600 && raw_filter < 3560)
                {
                    if (alarm_left == 0)
                    {
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);
                        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 170);
                    }
                }
                else
                {
                    if (alarm_right == 0)
                    {
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET);
                        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 170);
                    }
                }
            }
            // 起摆＋稳摆
            // 起摆
            if (start_mode == -1)
            {
                c++;
                if (c == 120)
                {
                    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
                    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET);
                    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 400);
                }
                if (c == 240)
                {
                    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
                    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);
                    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 400);
                }
                if (c == 360)
                {
                    start_mode = 0;
                    c = 0;
                }
            }
            // 开始超级起摆
            if (start_mode == 0)
            {
                // 判断是否进入低速起摆
                if ((raw_filter > 100 && raw_filter < 200) || (raw_filter > 2900 && raw_filter < 3000))
                {
                    start_mode = 1;
                }
                if (raw_filter > 1600 && raw_filter < 3560)
                {
                    if (alarm_left == 0)
                    {
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);
                        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 300);
                    }
                }
                else
                {
                    if (alarm_right == 0)
                    {
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET);
                        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 300);
                    }
                }
            }
            // 低速起摆
            if (start_mode == 1)
            {
                // 判断是否进入稳摆
                if (raw_filter > 3250 && raw_filter < 3450)
                {
                    start_mode = 2;
                }
                if (raw_filter > 1600 && raw_filter < 3560)
                {
                    if (alarm_left == 0)
                    {
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);
                        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 170);
                    }
                }
                else
                {
                    if (alarm_right == 0)
                    {
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET);
                        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 170);
                    }
                }
            }
            // 稳摆
            if (start_mode == 2 || flag_jc3 == 1)
            {
                // 掉下来重新起摆
                if (raw_filter < 3250 || raw_filter > 3450)
                {
                    start_mode = 1;
                }
                if (flag_stop == 0 && flag_init == 0)
                {
                    // 死区控制
                    if (angle_out < 40 && angle_out > 0)
                        angle_out = 40;
                    if (angle_out > -40 && angle_out < 0)
                        angle_out = -40;

                    if (angle_out > 0)
                    {
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);
                        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, angle_out);
                    }
                    if (angle_out < 0)
                    {
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
                        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET);
                        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, -angle_out);
                    }
                }
            }

            if (++time_1s == 100)
            {
                HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
                time_1s = 0;
            }
            a = 1;
        }
    }
}

char rxdatas[30];
uint8_t rxdata;
unsigned char rx_pointer;

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    rxdatas[rx_pointer++] = rxdata - 48;
    HAL_UART_Receive_IT(&huart3, &rxdata, 1);
}
